﻿using System;
using System.Threading;
using MLRobotic;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;

using MLRobotic.IA.Drivers;

using System.Collections;
using System.IO.Ports;
using System.Text;
using Devantech.Hardware.SD20;

namespace MLRobotic.IA
{
    public class Program
    {

        public static COM comAsserv;
        public static COM comBalise;
        public static SD20ServoDriver sd21;

        public static DateTime stoptime;
        public static DateTime waitime;

        public static void Init()
        {
            Debug.EnableGCMessages(false);
            Thread.Sleep(2000);
            Robot.Actionneurs.InitMoteur.Write(true);
            comAsserv = new COM("COM2");
            comBalise = new COM("COM1");
            //Log.Init();
            sd21 = new SD20ServoDriver();
            Capteurs.Init();
            AireDeJeu.Init();
            Robot.Actionneurs.ReplieServos();     
            
        }

        public static void Main()
        {
            
            Init();
            //Robot.couleur = Robot.RobotCouleur.Bleu;

            #region Procedure de depart

            if (Robot.couleur == Robot.RobotCouleur.Rouge)
            {
                Robot.currentPosition.X = 3000- AireDeJeu.Decalage-100;
                Robot.currentPosition.Y = 1410 + AireDeJeu.Decalage; 
                Robot.currentPosition.Teta = System.Math.PI+0.01;
            }
            else
            {
                Robot.currentPosition.X = 100 + AireDeJeu.Decalage;
                Robot.currentPosition.Y = 1410 + AireDeJeu.Decalage;
                Robot.currentPosition.Teta = 0.01;
            }
            Program.comAsserv.SetPosition(Robot.currentPosition.X, Robot.currentPosition.Y, Robot.currentPosition.Teta);




            #endregion

            
            Log.Write("PGM : Attente tirette");
            AireDeJeu.SetBougies(ushort.MaxValue);
            while (!Robot.Sensors.JackDepartDedans)
            {
                Thread.Sleep(10);
                Position.Process();
                comAsserv.Process();
                comBalise.Process();
            }
            Log.Write("PGM : Go");
            Robot.MatchDemarre = true;
            comBalise.Send(COM.typePacket.MatchCommence);
            stoptime = DateTime.Now.AddSeconds(90);
            
            Robot.currentCible = new Position(400, true);
            while (DateTime.Now < stoptime)
            {
                try
                {
                    IA.Process();
                    Capteurs.Process();
                    Position.Process();
                    comAsserv.Process();
                    comBalise.Process();
                    AireDeJeu.Process();
                    Servomoteur.Process();
                }
                catch (Exception ex)
                {
                    Log.Write("Exception : " + ex.Message);
                }
            }
            Robot.Actionneurs.InitMoteur.Write(false);
            stoptime = stoptime.AddSeconds(25);
            Robot.Actionneurs.PompeBallon.Write(true);
            while (DateTime.Now < stoptime)
            {

                Thread.Sleep(10);
            }
            Robot.Actionneurs.PompeBallon.Write(false);
        }

    }
}
